function [psi, theta, phi] = dcm2angle_ont(CMF, rollOnSingu) %#codegen
%DCM2ANGLE_ONT 将方向余弦矩阵转换为欧拉角
%
% Input Arguments
% # CMF: 3*3*m矩阵，方向余弦矩阵，由F系绕Z轴旋转psi，绕Y轴旋转theta，绕X轴旋转phi得到M系
% # CMF用欧拉角表示为（由F系绕Z轴旋转psi，绕Y轴旋转theta，绕X轴旋转phi得到M系）
% [ cos(psi)*cos(theta), cos(psi)*sin(phi)*sin(theta) - cos(phi)*sin(psi), sin(phi)*sin(psi) + cos(phi)*cos(psi)*sin(theta)]
% [ cos(theta)*sin(psi), cos(phi)*cos(psi) + sin(phi)*sin(psi)*sin(theta), cos(phi)*sin(psi)*sin(theta) - cos(psi)*sin(phi)]
% [         -sin(theta),                               cos(theta)*sin(phi),                               cos(phi)*cos(theta)]
% # rollOnSingu: 标量，当theta处于奇异区间（-pi/2或pi/2附近）时默认的phi，单位rad，默认为0
%
% Output Arguments:
% # psi: 长度为m的列向量，绕Z轴，值域(-pi, pi]，单位rad
% # theta: 长度为m的列向量，绕Y轴，值域[-pi/2, pi/2]，单位rad
% # phi: 长度为m的列向量，绕X轴，值域(-pi, pi]，单位rad
%
% References:
% # 《应用导航算法工程基础》 “方向余弦矩阵转换为欧拉角”

if nargin < 2
    rollOnSingu = 0;
end

p = size(CMF, 3);
phi = NaN(p, 1);
theta = NaN(p, 1);
psi = NaN(p, 1);

for i=1:p
    theta(i, 1) = atan2(-CMF(3, 1, i), sqrt(CMF(3, 2, i)^2+CMF(3, 3, i)^2));
    if CMF(3, 1, i) <= -(1-eps)
        phi(i, 1) = rollOnSingu;
        psi(i, 1) = atan2(CMF(2, 3, i)-CMF(1, 2, i), CMF(1, 3, i)+CMF(2, 2, i)) + phi(i, 1);
    elseif CMF(3, 1, i) >= (1-eps)
        phi(i, 1) = rollOnSingu;
        psi(i, 1) = atan2(-CMF(2, 3, i)-CMF(1, 2, i), -CMF(1, 3, i)+CMF(2, 2, i)) - phi(i, 1);
    else
        phi(i, 1) = atan2(CMF(3, 2, i), CMF(3, 3, i));
        psi(i, 1) = atan2(CMF(2, 1, i), CMF(1, 1, i));
    end
end
end